#include <stdio.h>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>

#include "opencv2/core.hpp"
#include "opencv2/imgproc.hpp"
#include "opencv2/highgui.hpp"
#include <opencv2/video/video.hpp>
#include <opencv2/opencv.hpp>
#include "opencv2/core/eigen.hpp"

#include "camera_class.h"
#include "kalman.hpp"
#include <iostream>
#include <cmath>
#include <cstdlib>

#define Light_width 13
#define Light_height 5.5

bool color_mode = true;

double dis(Point p1, Point p2);
void sort_point(Point2f p[]);
float get_angle(Point2f p[]);

use_kalman_filter KF;

vector<Point3f> obj_points_simple{     //定义装甲板世界点位 simple
    {-Light_width / 2, -Light_height / 2, 0},
    {-Light_width / 2, Light_height / 2, 0},
    {Light_width / 2, Light_height / 2, 0},
    {Light_width / 2, -Light_height / 2, 0}
};

vector<Point3f> obj_points_hero{     //定义装甲板世界点位 hero
    {-Light_width / 1.2, -Light_height / 2, 0},
    {-Light_width / 1.2, Light_height / 2, 0},
    {Light_width / 1.2, Light_height / 2, 0},
    {Light_width / 1.2, -Light_height / 2, 0}
};

//定义相机内参矩阵
Mat camera_matrix = (Mat_<float>(3,3) << 
    785.265 ,          0,   304.7549,
           0,   777.0345,   241.9729,
           0,          0,          1);

//定义相机畸变系数矩阵  k1, k2, p1, p2, k3
Mat distCoeffs = (Mat_<float>(1,5) << 
    -0.0932, 0.6745,
    -0.0015, -6.426517731358665e-05,
    -2.0820
);

double ptz_camera_x = 0.0;     // PTZ_CAMERA_X - 相机与云台的 X 轴偏移(左负右正) 【相机作为参考点 单位 m】
double ptz_camera_y = 0.045;     // PTZ_CAMERA_Y - 相机与云台的 Y 轴偏移(上负下正)
double ptz_camera_z = -0.075;  // PTZ_CAMERA_Z - 相机与云台的 Z 轴偏移(前正后负)
static double theta = 0;       // 定义相机旋转角度

static double r_data[] = {  1,           0,           0,  //rotation
                            0,  cos(theta),  sin(theta),
                            0, -sin(theta), cos(theta)};
// 定义平移矩阵
static double t_data[] = {static_cast<double>(ptz_camera_x),        //translation
                          static_cast<double>(ptz_camera_y),
                          static_cast<double>(ptz_camera_z)};


//装甲板识别模块类
class AngleSolve{

public:
    AngleSolve(Mat image);

    void AngleRes(vector<Point2f> img_points);

    Eigen::Vector3d pnp_get_pc(vector<Point2f> img_points);

    Mat Img_pre_dis(Mat image);
    void Img_getb(Mat image);

    float yaw_get();
    float pitch_get();
    Mat img_get();

private:
    Mat img;
    float m_yaw;
    float m_pitch;
};